#include "Serial.h"

#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <errno.h>

#include <iostream>
#include <fstream>
#include <string>
#include<string.h>
#include <stdexcept>
#include<exception>

#include<opencv2/opencv.hpp>


#include "../General/General.h"

using namespace std;

namespace rm
{
Serial::Serial():
    _serialFd(-1),
    _errorCode(OJBK),
    _en_debug(false),
    _lastRecordSeq(0)
{
    static_assert(sizeof(ControlFrame) == 16, "Size of backdata is not 16");
    static_assert(sizeof(FeedBackFrame) == 20, "Size of backdata is not be 20");

    // 初始化通讯帧结构体通用项
    _controlFrame.SOF = JetsonCommSOF;
    _controlFrame.frame_seq = 0;
    _controlFrame.EOF = JetsonCommEOF;
}

Serial::~Serial()
{
    tcflush(_serialFd, TCIOFLUSH);
    if (-1 == close(_serialFd))
	{
        _errorCode = SYSTEM_ERROR;
        cout << "Serial closing  failed." << endl;
    }
    else
    {
        _errorCode = OJBK;
    }
}

int Serial::openPort()
{
    _serialFd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NONBLOCK);//dev/ttyUSB0
     if (_serialFd == -1)
    {
        cout << "Open serial port0 failed." << endl;
        _serialFd = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NONBLOCK);//dev/ttyUSB1
        if(_serialFd == -1)
        {
            cout << "Open serial port1 failed." << endl;
            return _errorCode = SYSTEM_ERROR;
        }
    }

    termios tOption;                                // 串口配置结构体
    tcgetattr(_serialFd, &tOption);                 //获取当前设置
    cfmakeraw(&tOption);
    cfsetispeed(&tOption, B460800);                 // 接收波特率
    cfsetospeed(&tOption, B460800);                 // 发送波特率
    tcsetattr(_serialFd, TCSANOW, &tOption);
    tOption.c_cflag &= ~PARENB;
    tOption.c_cflag &= ~CSTOPB;
    tOption.c_cflag &= ~CSIZE;
    tOption.c_cflag |= CS8;
    //tOption.c_cflag &= ~INPCK;//
    tOption.c_cflag |= (CLOCAL | CREAD);  // 设置波特率，本地连接，接收使能
    //tOption.c_cflag &= ~(INLCR | ICRNL);// == tOption.c_cflag &= ~(PARENB | CSTOPB);
    //tOption.c_cflag &= ~(IXON);//
    tOption.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    tOption.c_oflag &= ~OPOST;
    tOption.c_oflag &= ~(ONLCR | OCRNL);
    tOption.c_iflag &= ~(ICRNL | INLCR);
    tOption.c_iflag |= IGNBRK;
    tOption.c_iflag &= ~(IXON | IXOFF | IXANY);
    tOption.c_cc[VTIME] = 0; //                       //只有设置为阻塞时这两个参数才有效
    tOption.c_cc[VMIN] = 0;//
    tcflush(_serialFd, TCIOFLUSH);                  //TCIOFLUSH刷新输入、输出队列。

    cout << "Serial preparation complete." << endl;
    return _errorCode = OJBK;
}

int Serial::closePort()
{
    tcflush(_serialFd, TCIOFLUSH);
    if (-1 == close(_serialFd))
    {
        _errorCode = SYSTEM_ERROR;
        cout << "Serial closing failed." << endl;
    }
    else
    {
        _errorCode = OJBK;
    }
    return _errorCode;
}

bool Serial::isOpened() const
{
    return (_serialFd != -1);
}

void Serial::setDebug(bool en_debug)
{
    _en_debug = en_debug;
}

int Serial::setup(int& self_color)
{
    if(_en_debug)
    {
        cout << "[setup]\n";
    }

    u_int16_t tmp_self_team;
    _controlFrame.shoot_mode = SET_UP;
    //print(_controlFrame);
    if(send() == OJBK)
    {
        cout<<"hello 7"<<endl;
        if(receive() == OJBK)
        {
            cout<<"hello 8"<<endl;

            //tmp_self_team = ((_feedBackFrame.task_mode << 8) | _feedBackFrame.bullet_speed);
            tmp_self_team = _feedBackFrame.remain_HP;
            cout<<hex<<_feedBackFrame.remain_HP<<endl;
            self_color = (_feedBackFrame.remain_HP == 0xEEEE) ? RED_TEAM : BLUE_TEAM;
            cout<<hex<<self_color<<endl;
            if(self_color != BLUE_TEAM && self_color != RED_TEAM)
            {
                return _errorCode = CORRUPTED_FRAME;
            }
            _controlFrame.shoot_mode = tmp_self_team;


//---------------stupid stm32 is so slow that we have to wait about 2ms here on NUC
//                for(int i=0;i<50000000;i++)
//                {

//                }
//                cout<<"1 turn"<<endl;
//------------------------------------------------------------------------------

            if(send() == OJBK)
            {
//                cout<<"hello 9"<<endl;
//                cout<<"-----------controlframe----------------\n";
//                print(_controlFrame);
//                cout<<"---------------------------------------\n";
                if(self_color == BLUE_TEAM)
                {
                    self_color = rm::BLUE;
                }
                else if(self_color == RED_TEAM)
                {
                    self_color = rm::RED;
                }
            }

        }
        //print(_feedBackFrame);
    }
    return _errorCode;
}

int Serial::record(u_int8_t& frame_seq, GyroData & gyroData)
{
    if(_en_debug)
    {
        //cout << "[record]\n";
    }

    _lastRecordSeq++;
    _controlFrame.frame_seq = _lastRecordSeq;
    _controlFrame.shoot_mode = RECORD_ANGLE;

    frame_seq = _lastRecordSeq;

//====================================
    if(send() == OJBK)
    {
        if (receiveFromGyro() == OJBK)
        {
            gyroData = unpack(_gyroFrame);
        }
        else
        {
            gyroData.gimbal_pitch = 0;
            gyroData.gimbal_yaw = 0;
        }
    }
    //print(_gyroFrame);

    //cout<<sizeof(GyroDataFrame)<<"!"<<endl;
    //cout<<sizeof(FeedBackFrame)<<"!"<<endl;
//====================================
    return _errorCode;
//    gyroData.gimbal_pitch = 0;
//    gyroData.gimbal_yaw = 0;
//    return send();
}

int Serial::control(const ControlData& controlData)
{ //tcflush(_serialFd, TCOFLUSH);
    if(_en_debug)
    {
        //cout << "[control]\n";
    }

    _controlFrame = pack(controlData);

    print(_controlFrame);
    return send();
}

//int Serial::debug(const ControlData& controlData)
//{
//    if(_en_debug)
//    {
//        cout << "[debug]\n";
//    }
//    cout << "[debug]\n";
//    _controlFrame = pack(controlData);
//    if(send() == OJBK)
//    {
//        if (receive_longtime() == OJBK)
//        {
//            FeedBackData feedBackData;
//            feedBackData = unpack(_feedBackFrame);
//        }
//    }
//    return _errorCode;
//}

int Serial::feedBack(FeedBackData& feedBackData)
{
    if(_en_debug)
    {
        //cout << "[request]\n";
    }

    _controlFrame.shoot_mode = REQUEST_TRANS;
    if(send() == OJBK)
    {
        if (receive() == OJBK)
        {
            feedBackData = unpack(_feedBackFrame);
        }
    }
    //print(_feedBackFrame);
    return _errorCode;
//    return send();
}

int Serial::getErrorCode() const
{
    return _errorCode;
}


void Serial::print(const ControlFrame &ct)
{
//    cout<<"================control=============="<<endl;
//    cout<<hex<<(unsigned int)ct.SOF<<endl;
//    cout<<dec<<(unsigned int)ct.frame_seq<<endl;
//    cout<<hex<<(unsigned int)ct.shoot_mode<<endl;
//    cout<<"sb is a sb"<<endl;
//    cout<<dec<<              ct.pitch_dev<<endl;
//    cout<<dec<<              ct.yaw_dev<<endl;
//    cout<<"yaw is pitch"<<endl;
//    cout<<dec<<         (int)ct.rail_speed<<endl;
//    cout<<hex<<(unsigned int)ct.gimbal_mode<<endl;
//    cout<<hex<<(unsigned int)ct.EOF<<endl;
//    cout<<"================CONTROL=============="<<endl;
}

void Serial::print(const FeedBackFrame &fb)
{
    cout<<"=================feedback=================="<<endl;
    cout<<hex<<(unsigned int)fb.SOF<<endl;
    cout<<hex<<(unsigned int)fb.frame_seq<<endl;
    cout<<hex<<(unsigned int)fb.task_mode<<endl;
    cout<<hex<<(unsigned int)fb.bullet_speed<<endl;
    cout<<hex<<(unsigned int)fb.rail_pos<<endl;
    cout<<hex<<(unsigned int)fb.shot_armor<<endl;
    cout<<hex<<(unsigned int)fb.remain_HP<<endl;
    for(int i = 0;i<3;i++)
    {
        cout<<hex<<(unsigned int)(fb.reserved[i])<<", ";
    }
    cout<<endl;
    cout<<dec<<(float)fb.gimbal_pitch<<endl;
    cout<<dec<<(float)fb.gimbal_yaw<<endl;
    cout<<hex<<(unsigned int)fb.EOF<<endl;
    cout<<"=================FEEDBACK=================="<<endl;
}

void Serial::print(const GyroDataFrame &gd)
{
    cout<<"=================record=================="<<endl;
    cout<<hex<<(unsigned int)gd.SOF<<endl;
    cout<<dec<<(unsigned int)gd.frame_seq<<endl;
    cout<<dec<<(float)gd.gimbal_pitch<<endl;
    cout<<dec<<(float)gd.gimbal_yaw<<endl;
    cout<<hex<<(unsigned int)gd.EOF<<endl;
    cout<<"=================RECORD=================="<<endl;
}

Serial::ControlFrame Serial::pack(const ControlData& ctrl)
{
    return ControlFrame
    {
        JetsonCommSOF,
        ctrl.frame_seq,
        ctrl.shoot_mode,
        ctrl.pitch_dev,
        ctrl.yaw_dev,
        ctrl.rail_speed,
        ctrl.gimbal_mode,
        JetsonCommEOF
    };
}

FeedBackData Serial::unpack(const Serial::FeedBackFrame& feedBackData)
{
    return FeedBackData
    {
        feedBackData.task_mode,
        feedBackData.bullet_speed,
        feedBackData.rail_pos,
        feedBackData.shot_armor,
        feedBackData.remain_HP,
        feedBackData.gimbal_pitch,
        feedBackData.gimbal_yaw
    };
}

GyroData Serial::unpack(const Serial::GyroDataFrame& gd)
{
    return GyroData
    {
        gd.gimbal_pitch,
        gd.gimbal_yaw
    };
}

int Serial::send()
{
    tcflush(_serialFd, TCOFLUSH);
    int sendCount;
    try
    {
       sendCount  = write(_serialFd, &_controlFrame, sizeof(ControlFrame));
    }
    catch(exception e)
    {
        cout << e.what() << endl;
        return _errorCode = SYSTEM_ERROR;
    }

    if (sendCount == -1)
	{
        if (_en_debug)
        {
            cout << "\tSerial sending failed. Frame sequence: " << (int)_controlFrame.frame_seq << endl;
        }
        _errorCode = READ_WRITE_ERROR;
	}
    else if (sendCount < static_cast<int>(sizeof(ControlFrame)))
	{
        if (_en_debug)
        {
            cout << "\tSerial sending failed. "<< sizeof(ControlFrame) - sendCount <<
                    " bytes unsent. Frame sequence: " << (int)_controlFrame.frame_seq << endl;
        }
        _errorCode = READ_WRITE_ERROR;
    }
    else
    {
        if (_en_debug)
        {
            //cout << "\tSerial sending succeeded. " << "Frame sequence: " << (int)_controlFrame.frame_seq <<" sent bytes:"<<sizeof(ControlFrame)<< endl;
        }

        _errorCode = OJBK;
    }
    return _errorCode;
}

int Serial::receive()
{
    memset(&_feedBackFrame,0,sizeof(_feedBackFrame));

    int readCount = 0;
    const auto t1 = std::chrono::high_resolution_clock::now();
    while (readCount < int(sizeof(FeedBackFrame)))
	{
        auto t2 = std::chrono::high_resolution_clock::now();
        if ((chrono::duration_cast<std::chrono::milliseconds>(t2 - t1)).count() > 10) // Time limit is 5ms
        {
            if(_en_debug)
            {
                cout << "\tReceiving time out. " << sizeof(FeedBackFrame) - readCount
                     << " bytes not received. Frame sequence: "<< (int)_feedBackFrame.frame_seq << endl;
            }

            return _errorCode = TIME_OUT;
        }

        int onceReadCount;
        try
        {
            onceReadCount = read(_serialFd, ((unsigned char *)(&_feedBackFrame)) + readCount, sizeof(FeedBackFrame) - readCount);
            //print(_feedBackFrame);
        }
        catch(exception e)
        {
            cout << e.what() << endl;
            return _errorCode = SYSTEM_ERROR;
        }

        if (onceReadCount == -1)
		{
            if (errno == EAGAIN)
			{
				continue;
			}

            if(_en_debug)
            {
                cout << "\tRead data from serial failed. Frame sequence: " << (int)_feedBackFrame.frame_seq << endl;
            }
            return _errorCode = READ_WRITE_ERROR;
		}

        readCount += onceReadCount;
	}

    tcflush(_serialFd, TCIFLUSH);

    if (_feedBackFrame.SOF != JetsonCommSOF || _feedBackFrame.EOF != JetsonCommEOF)
    {
        if (_en_debug)
        {
            cout << "\tFeed back frame SOF or EOF is not correct. SOF: " << (int)_feedBackFrame.SOF << " ,EOF: " << (int)_feedBackFrame.EOF << endl;
        }
        return _errorCode = CORRUPTED_FRAME;
    }
    else
    {
        if (_en_debug)
        {
            //cout << "\tSerial receiving succeeded. " << "Frame sequence: " << (int)_feedBackFrame.frame_seq << endl;
        }
        return _errorCode = OJBK;
    }
}

int Serial::receiveFromGyro()
{
    memset(&_gyroFrame,0,sizeof(_gyroFrame));

    int readCount = 0;
    const auto t1 = std::chrono::high_resolution_clock::now();
    while (readCount < int(sizeof(GyroDataFrame)))
    {
        auto t2 = std::chrono::high_resolution_clock::now();
        if ((chrono::duration_cast<std::chrono::milliseconds>(t2 - t1)).count() > 4) // Time limit is 4ms
        {
            if(_en_debug)
            {
                cout << "\tReceiving time out. " << sizeof(GyroDataFrame) - readCount
                     << " bytes not received. "<< endl; /*Frame sequence: << (int)_gyroFrame.frame_seq*/
            }
            return _errorCode = TIME_OUT;
        }

        int onceReadCount;
        try
        {
            onceReadCount = read(_serialFd, ((unsigned char *)(&_gyroFrame)) + readCount, sizeof(GyroDataFrame) - readCount);
        }
        catch(exception e)
        {
            cout << e.what() << endl;
            return _errorCode = SYSTEM_ERROR;
        }

        if (onceReadCount == -1)
        {
            if (errno == EAGAIN)
            {
                continue;
            }

            if(_en_debug)
            {
                cout << "\t gyro Read data from serial failed. " << endl;//Frame sequence:  << (int)_gyroFrame.frame_seq
            }
            return _errorCode = READ_WRITE_ERROR;
        }

        readCount += onceReadCount;
    }

    tcflush(_serialFd, TCIFLUSH);

//    if (_gyroFrame.SOF != JetsonCommSOF || _gyroFrame.EOF != JetsonCommEOF)
//    {
//        if (_en_debug)
//        {
//            cout << "\tFeed back frame SOF or EOF is not correct. SOF: " << (int)_gyroFrame.SOF << " ,EOF: " << (int)_feedBackFrame.EOF << endl;
//        }
//        return _errorCode = CORRUPTED_FRAME;
//    }
//    else
//    {
        if (_en_debug)
        {
            //cout << "\tSerial receiving succeeded. "  << endl;//"Frame sequence: " << (int)_gyroFrame.frame_seq<<
        }
        return _errorCode = OJBK;
//    }
}

/*int Serial::tryPushBackControl(ControlData &controlData, int controlMode)
{
    if(controlMode == RECORD_ANGLE)
    {
        _lastRecordSeq++;
        controlData.frame_seq = _lastRecordSeq;
        controlData.shoot_mode = RECORD_ANGLE;
        //frame_seq = _lastRecordSeq;
    }
    else if(controlMode == SET_UP)
    {
        controlData.shoot_mode = SET_UP;
    }
    else if(controlMode == REQUEST_TRANS)
    {
        controlData.shoot_mode = REQUEST_TRANS;
    }
    else
    {
        //control frame
    }

    unique_lock<mutex> lock(_buffer_mutex);
    if(!lock.owns_lock())
    {
//        usleep(500);
//        lock.lock();
        return PORT_OCCUPIED;//buffer occupied
    }
    if(_controlDataBuffer.size() >= _max_buffer_size)
    {
        _controlDataBuffer.pop_front();
    }
    _controlDataBuffer.emplace_back(controlData);

    return OJBK;
}

int Serial::trySend(FeedBackData &feedBackData)
{
    unique_lock<mutex> lock(_buffer_mutex);
    while(!lock.owns_lock())
    {
        usleep(200);
        lock.lock();
    }
    if(_controlDataBuffer.size() > 0)
    {
        if(_controlDataBuffer[0].shoot_mode == SET_UP)
        {
            u_int16_t tmp_self_team;
            _controlFrame.shoot_mode = SET_UP;
            if(send() == OJBK)
            {
                if(receive() == OJBK)
                {
                    tmp_self_team = ((_feedBackFrame.task_mode << 8) | _feedBackFrame.bullet_speed);
                    if(tmp_self_team != BLUE_TEAM && tmp_self_team != RED_TEAM)
                    {
                        return _errorCode = CORRUPTED_FRAME;
                    }
                    _controlFrame.shoot_mode = tmp_self_team;
                    if(send() == OJBK)
                    {
                        if(tmp_self_team == BLUE_TEAM)
                        {
                            feedBackData.task_mode = rm::BLUE;
                        }
                        else if(tmp_self_team == RED_TEAM)
                        {
                            feedBackData.task_mode = rm::RED;
                        }

                    }
                }
            }
            return _errorCode;
        }
        else if(_controlDataBuffer[0].shoot_mode == RECORD_ANGLE)
        {
            //gyroData.gimbal_pitch = 0;
            //gyroData.gimbal_yaw = 0;
            _controlFrame.shoot_mode = RECORD_ANGLE;
            _controlDataBuffer.pop_front();
            return send();
        }
        else if(_controlDataBuffer[0].shoot_mode == REQUEST_TRANS)
        {
            _controlFrame.shoot_mode = REQUEST_TRANS;
            if(send() == OJBK)
            {
                if (receive() == OJBK)
                {
                    feedBackData = unpack(_feedBackFrame);
                }
            }
            _controlDataBuffer.pop_front();
            return _errorCode;
        }
        else//control frame
        {
            _controlFrame = pack(_controlDataBuffer[0]);
            _controlDataBuffer.pop_front();
            return send();
        }
    }
    return OJBK;
}*/

//int trySendRecord(uint8_t seq, GyroData & gyroData)
//{
//    unique_lock<mutex> lock(_buffer_mutex);
//    while(!lock.owns_lock())
//    {
//        usleep(200);
//        lock.lock();
//    }


//}



}
